/**
*/


#include <OGF/quick_start/common/skeleton.h>
#include <OGF/quick_start/common/select_util.h>
#include <OGF/basic/os/stopwatch.h>
#include <OGF/cells/map/map_cell_heap.h>

#include <set>
#include <stack>
#include <queue>
#include <limits>

#include <OGF/math/linear_algebra/vector.h>

using namespace std;

namespace OGF 
{
	Skeleton::Skeleton()
	{
		simplifier = nil;
		solver = nil;
		surface = nil;
		skeleton = nil;

	}
	Skeleton::~Skeleton()
	{
	}

	/**
	* Do init, but only if the surface has changed.
	* Checks is the surface has changed by pointer equality,
	* and number of verts and half-edges.
	*/
	void Skeleton::lazy_init(Surface *surface)
	{	
		//Properties we watch on the mesh:
		static int num_verts = -1;
		static int num_edge = -1;

		//if the surface has changed since the last time we ran init(),
		//then run it again.
		if(surface != this->surface 
			|| num_verts != surface->size_of_vertices() 
			|| num_edge != surface->size_of_halfedges())
		{
			num_verts = surface->size_of_vertices();
			num_edge = surface->size_of_halfedges();

			init(surface); 
		}
	}


	void Skeleton::init(Surface *surface)
	{
		this->surface = surface;
		
		index = new MapVertexAttribute<int>(surface, "index");
		MapVertexAttribute<double> initOneRingArea(surface, "VERTEX_AREA");
		MapVertexAttribute<Point3d> previousVertexPosition(surface, "VERTEX_POSITION");
		MapVertexAttribute<Point3d> originalVertexPosition(surface, "ORGINIAL_POSITION");

		// Calculate the vertex positions
		// Assign an index to each vertex
		int i = 0;
		FOR_EACH_VERTEX(Map, surface, v)
		{
			Point3d p = v->point();
			previousVertexPosition[v] = p;
			originalVertexPosition[v] = p;
			(*index)[ v ] = i;
			i++;
		}

		// Calculate original local areas
		FOR_EACH_VERTEX(Map, surface, v){
			initOneRingArea[v] = calc_local_area(v);
		}

		// Calculate average face area for model
		Aavg = Geom::map_area(surface)/surface->size_of_facets();

		// Set starting weights
		WL = 1.0;	// Starting Contraction weight (paper gives this as 10^-3 * sqrt(Aavg) )
		WH_0 = 1.0;	// Starting Attraction weight

		// Set required constants
		MAX_CONTRACTION_SQUARED = 100000.;	// Maximum contraction weight^2 before using hard constraint (default 100000)
		MAX_ATTRACTION = 100000000.;		// Maximum attraction weight before using hard constraint    (default 1000)
		MIN_COT_ANGLE = 0.00000000000000001;	// Minimum angle for cotan weights before clamping           (default 0.0000001)
		CONTRACTION_SCALE = 2;			// Iteration growth of contraction weight                    (default 4)
			
	}

	void Skeleton::extract_iteration(Surface* s) {

		lazy_init(s);

		MapVertexAttribute<double> initOneRingArea(surface, "VERTEX_AREA");
		MapVertexAttribute<Point3d> previousVertexPosition(surface, "VERTEX_POSITION");

		

		// Initialize the system solver - one diff solver per iteration
		delete solver;
        	SystemSolverParameters params ;
        	params.set_arg("method", "SUPERLU") ; 
		solver = new LinearSolver(surface->size_of_vertices()) ;
		solver->set_system_solver(params) ;
		solver->set_least_squares(true) ;
		solver->set_invert_matrix(true) ;

		calculate_cotan_weights();

		//MapVertexAttribute<double> isDegenerate(surface, "isdeg");
		// Setup and solve the system for each coordinate
		for(int coord=0 ; coord < 3 ; coord++){
			
			// Lock any vertices which are nearly degenerate
			// we don't want to soft constrain them with arbitrarily 
			// large weights
			FOR_EACH_VERTEX(Map, surface, vert)
			{
				if(isDegenerateVertex(vert)){
					solver->variable((*index)[vert]).lock();
				}
				double r = previousVertexPosition[vert][coord];
				solver->variable((*index)[vert]).set_value(r);		
			}

			solver->begin_system();

			// Form the Laplacian matrix
			// Use cotangent weights with each row scaled by the current attraction constant
			FOR_EACH_VERTEX(Map, surface, vert)
			{

			if(!isDegenerateVertex(vert)){
				solver->begin_row();

				int vertIndex = (*index)[vert];
				double w_ii = 0.0;
				Map::Halfedge *he;

				

				FOR_EACH_ADJACENT_OUTEDGE(vert, he)
				{
					double w_ij = (get_cotan_weight(he));
					w_ii += get_cotan_weight(he);
					solver->add_coefficient((*index)[he->vertex()], WL*w_ij);
				}
				solver->add_coefficient(vertIndex, -WL*w_ii);
				solver->set_right_hand_side(0.0);

				solver->end_row();
			}
			}

			// Form the Attraction Constraints matrix
			FOR_EACH_VERTEX(Map, surface, vert)
			{
				// Only form this soft constrain row if we don't already have a hard
				// constraint on this vertex position
				if(!isDegenerateVertex(vert)){
					
					solver->begin_row();

					// Update the weight proportional to sqrt(A0/A)
					// Where A0 is the original vertex one-ring area
					// And A is the current vertex one-ring area
					double A = calc_local_area(vert);			
					double w = WH_0*sqrt(initOneRingArea[vert]/A);				
					solver->add_coefficient((*index)[vert],w);
					solver->set_right_hand_side(w * previousVertexPosition[vert][coord]);
				
					solver->end_row();
				}
			}

			solver->end_system();

			solver->solve();

			// Update the vertex positions
			FOR_EACH_VERTEX(Map, surface, vert) {
				double r = solver->variable((*index)[vert]).value();
				previousVertexPosition[vert][coord] = r;
			}

			solver->restart();

		} //end coord

		// Update weights
		WL = WL * CONTRACTION_SCALE;

		// For now update the real vertex position too
		FOR_EACH_VERTEX(Map, surface, vert)
		{
			vert->set_point(previousVertexPosition[vert]);
		}

	}


	/*
	 * Return the cotan weight for a given edge
	 * cot (alpha ij) + cot (beta ij)
	 */
	double Skeleton::get_cotan_weight(Map::Halfedge* he) {
		MapHalfedgeAttribute<double> cotanWeights(surface, "COTAN_WEIGHTS");
		
		return cotanWeights[he] + cotanWeights[he->opposite()];
	}

	/*
	 * Calculate the cotangent of the opposite angle to each halfedge
	 * Store in halfedge attribute "COTAN_WEIGHTS"
	 */
	void Skeleton::calculate_cotan_weights() {
		MapHalfedgeAttribute<double> cotanWeights(surface, "COTAN_WEIGHTS");
		
		Map::Halfedge* he;
		FOR_EACH_HALFEDGE(Map, surface, he) {
			// Calculate outside angle for this halfedge
			Point3d A = he->opposite()->vertex()->point();
			Point3d B = he->vertex()->point();
			Point3d C = he->next()->vertex()->point();
			Vector3d v1 = B-C;
			Vector3d v2 = A-C;
			double angle = Geom::angle(v1,v2);
			// Handle degenerate angles. This may not be necessary because of locking
			if(angle < MIN_COT_ANGLE)
				angle = MIN_COT_ANGLE;

			// Set the cotan weight
			cotanWeights[he] = 1.0/tan(angle);
		}	
	}

	/*
	 * Return true if the vertex is nearly degenerate
	 * Basically check if its associated weights will be too high
	 */
	bool Skeleton::isDegenerateVertex(Map::Vertex* v)
	{
		MapVertexAttribute<double> initOneRingArea(surface, "VERTEX_AREA");
		MapHalfedgeAttribute<double> degenerate(surface, "d"); //testing

		// Check if the attraction weight for any neighbours is too high
		// If this neighbour gets locked then we will have to take all the 
		// responsibility, possibly giving ugly results
		Map::Facet *f;
		FOR_EACH_ADJACENT_FACE(v, f)
		{
			double A = Geom::facet_area(f);
			if(Aavg/MAX_CONTRACTION_SQUARED > A){
				degenerate[v->halfedge()] = 1000.;
				return true;
			}
		}
		/*double A = calc_local_area(v);
		if(Aavg/MAX_CONTRACTION_SQUARED > A/6.)
				return true;
		*/
		// Check if the contraction weight towards any neighbour is too high
		Map::Halfedge *he;
		FOR_EACH_ADJACENT_OUTEDGE(v, he)
		{
			if(get_cotan_weight(he) > MAX_ATTRACTION){
				degenerate[v->halfedge()] = 500.;
				return true;
			}
		}

		return false;

	}




	/*
	 * Return the area of all the faces surrounding this vertex
	 */
	double Skeleton::calc_local_area(Map::Vertex *v)
	{	
		Map::Facet *f;
		double area = 0;
		FOR_EACH_ADJACENT_FACE(v, f)
		{
			ogf_assert(f != nil);
			area += Geom::facet_area(f);
		}

		return area;
	}


	Line* Skeleton::extractSkeletonGraph() {
        	Line* graph = Line::find_or_create(surface->scene_graph(), "Skeleton") ;
        	GraphEditor editor(graph) ;
        	MapVertexAttribute<Graph::Vertex*> vtov(surface) ;

		MapVertexAttribute<Graph::Vertex*> skeletonVertex(surface, "skeletonVertex"); // Each map vertex points to one skeleton vertex
		GraphVertexAttribute<vector<Map::Vertex*> > mapVertices(graph, "mapVertices"); // Each skeleton vertex points to a list of map vertices

		// Copy vertex positions and set correspondence attributes
        	{FOR_EACH_VERTEX(Map, surface, it) {
            		Graph::Vertex* v = editor.new_vertex(it->point()) ;
            		vtov[it] = v ;
			skeletonVertex[it] = v;
			mapVertices[v].push_back(it);
        	}}
		// Copy connectivity
        	{FOR_EACH_EDGE(Map, surface, it) {
	            	Graph::Halfedge* graph_edge = editor.connect_vertices(
        	        vtov[it->vertex()],
                	vtov[it->opposite()->vertex()]) ;
        	}}
        	
		return graph;
    	}

	void Skeleton::initializeSimplifier(Surface* s)
	{
		lazy_init(s);
	
		//delete simplifier;
		//delete skeleton;


		// Setup the Skeleton
		if(simplifier == nil)
			simplifier = new Simplify();
		skeleton = extractSkeletonGraph();
		simplifier->lazy_init(skeleton, surface);
	
		skeleton->update();

		// Pop the surface back in place
		MapVertexAttribute<Point3d> originalVertexPosition(surface, "ORGINIAL_POSITION");
		MapVertexAttribute<Point3d> contractedVertexPosition(surface, "CONTRACTED_POSITION");
		FOR_EACH_VERTEX(Map, surface, it) {
			contractedVertexPosition[it] = it->point();
			it->set_point(originalVertexPosition[it]);
		}
		surface->update();


	}

	void Skeleton::simplifySingleStep()
	{
		if(simplifier == nil || skeleton == nil || surface == nil)
			return; 

		simplifier->simplifyMeshSingle();
		skeleton->update();
	}

	void Skeleton::simplify(int vertex_count)
	{

		if(simplifier == nil || skeleton == nil || surface == nil)
			return;

		simplifier->simplifyMesh(vertex_count);
		skeleton->update();
	}

	void Skeleton::setFeatureSize()
	{
		if(simplifier == nil || skeleton == nil || surface == nil)
			return;

		MapVertexAttribute<double> featureSize(surface, "FEATURE_SIZE");
		GraphVertexAttribute<vector<Map::Vertex*> > mapVertices(skeleton, "mapVertices"); 
		
		FOR_EACH_VERTEX(Graph, skeleton, it)
		{
			for(int i=0 ; i < mapVertices[it].size() ; i++){
				double size = (it->point() - mapVertices[it][i]->point()).norm();
				featureSize[ mapVertices[it][i] ] = size;
			}

		}


	}


} //namespace
